from machine import *
import math

class accel():
    def __init__(self, i2c, addr=0x68):
        self.iic = i2c
        self.addr = addr
        self.iic.start()
        self.iic.writeto(self.addr, bytearray([107, 0]))
        self.iic.stop()
        
        self.tim = Timer(0)
        self.tim.init(period=5, callback=self.timHandler)
        self.flag = False
        
        self.axo = 0
        self.ayo = 0
        self.azo = 0
        self.gxo = 0
        self.gyo = 0
        self.gzo = 0
        
        self.aax = 0
        self.aay = 0
        self.aaz = 0
        self.agx = 0
        self.agy = 0
        self.agz = 0
        
    def timHandler(self, t):
        if self.flag:
            try:
                self.kfilter()
            except:
                pass

    def get_raw_values(self):
        self.iic.start()
        a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
        self.iic.stop()
        return a

    def get_ints(self):
        b = self.get_raw_values()
        c = []
        for i in b:
            c.append(i)
        return c

    def bytes_toint(self, firstbyte, secondbyte):
        if not firstbyte & 0x80:
            return firstbyte << 8 | secondbyte
        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)

    def get_values(self):
        raw_ints = self.get_raw_values()
        vals = {}
        vals["AcX"] = self.bytes_toint(raw_ints[0], raw_ints[1])
        vals["AcY"] = self.bytes_toint(raw_ints[2], raw_ints[3])
        vals["AcZ"] = self.bytes_toint(raw_ints[4], raw_ints[5])
        vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
        vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])
        vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])
        vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])
        return vals  # returned in range of Int16
        # -32768 to 32767

    def val_test(self):  # ONLY FOR TESTING! Also, fast reading sometimes crashes IIC
        from time import sleep
        while 1:
            print(self.get_values())
            sleep(0.05)
            
           
    def mean(self, times):
        i = 0
        while i < times:
            ad = self.get_values()
            self.axo += ad["AcX"]
            self.ayo += ad["AcY"]
            self.azo += ad["AcZ"]
            self.gxo += ad["GyX"]
            self.gyo += ad["GyY"]
            self.gzo += ad["GyZ"]
            i += 1
        self.axo /= times
        self.ayo /= times
        self.azo /= times
        self.gxo /= times
        self.gyo /= times
        self.gzo /= times
        self.flag = True
           
           
    def kfilter(self):
        ad = self.get_values()
        # 加速度计
        ax = 2*9.8*ad["AcX"]/32768
        ay = 2*9.8*ad["AcY"]/32768
        az = 2*9.8*ad["AcZ"]/32768
        # 弧度制角度
        if az == 0 and ay >= 0:
            self.aax = 90
        elif az == 0 and ay < 0:
            self.aax = -90
        else:
            self.aax = math.atan(ay/az)*180/math.pi
        
        if az == 0 and ax >= 0:
            self.aay = -90
        elif az == 0 and ax < 0:
            self.aay = 90
        else:
            self.aay = -math.atan(ax/az)*180/math.pi
        
        if ay == 0 and az >= 0:
            self.aaz = -90
        elif ay == 0 and az < 0:
            self.aaz = 90
        else:
            self.aaz = -math.atan(az/ay)*180/math.pi
        # 角速度计
        self.agx = 1000*(ad["GyX"]-self.gxo)/32768
        self.agy = 1000*(ad["GyY"]-self.gyo)/32768
        self.agz = 1000*(ad["GyZ"]-self.gzo)/32768




